import sensor, image, time, math
from pyb import UART
from ulab import numpy as np
import json
import utime
from SF_Serial import MotorController


motor_controller = MotorController()  #创建电机控制对象
motor_controller.initialize_serial(port=3, baudrate=921600)  # 选择openmv的串口3，波特率921600


#yel_threshold = (54, 100, -27, 44, 40, 71)#黄色
#gre_threshold = (0, 100, -50, -15, 0, 45)#绿色
ora_threshold = (0, 100, 15, 55, 25, 55)#橙色



sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 格式为 RGB565.
sensor.set_framesize(sensor.QVGA) # 使用 QQVGA 速度快一些
sensor.skip_frames(time = 2000) # 跳过2000s，使新设置生效,并自动调节白平衡
sensor.set_auto_gain(False) # 关闭自动自动增益。默认开启的，在颜色识别中，一定要关闭白平衡。
#sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False)
sensor.set_vflip(True)
clock = time.clock() # 追踪帧率

def _constrain(amt,low,high):
    if amt<low:
        amt = low
    elif amt>high:
        amt = high
    return amt


#创建pid类
class PIDController:
    def __init__(self, P=0,  I=0,  D=0, limit=0):
        self.P = P
        self.I = I
        self.D = D
        self.limit = limit

        self.timestamp_prev = 0
        self.integral_prev = 0
        self.error_prev = 0

    def operator(self,error):
#        gloabal
        timestamp_now = utime.time()
        Ts = timestamp_now - self.timestamp_prev
        if Ts <= 0 or Ts > 0.5:
            Ts = 1e-3

        proportional = self.P * error
        integral = self.integral_prev + self.I*Ts*0.5*(error + self.error_prev)
        integral = _constrain(integral, -self.limit, self.limit)
        derivative = self.D*(error - self.error_prev)/Ts

        output = proportional + integral + derivative
        output = _constrain(output, -self.limit, self.limit)

        self.integral_prev = integral
        self.output_prev = output
        self.error_prev = error
        self.timestamp_prev = timestamp_now
        return output

#创建pid对象
angle_loop = PIDController(P=1.7,I=0.1,limit=8) #位置环
inertia_loop =  PIDController(P=0.6,limit=3)    #大减速环
Minertia_loop = PIDController(P=0.1,limit=8)    #小减速环

#返回看到的最大色块
def find_max(blobs):
    max_size=0
    for blob in blobs:
       if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

prev_time = 0
prev_X_targe = 0
while(True):
    clock.tick()
    img = sensor.snapshot() # 从感光芯片获得一张图像
    blobs = img.find_blobs([ora_threshold],area_threshold  = 240, pixels_threshold  = 240, merge = True)
    if blobs:
        max_blob = find_max(blobs)#返回看到的最大色块
        rect = max_blob.rect()
        x, y, w, h = rect[0], rect[1], rect[2], rect[3]  #色块左上角x,y坐标和长宽
        img.draw_rectangle(rect, color = (255, 0, 0))    #在图像中画框
        X_targe = (x+w/2-160)*0.006    #将目标X坐标转为以图像中心为原点的坐标轴坐标，并将坐标转为大致角度值

    #   pid计算
        now_time = utime.time()    #获取当前时间戳
        Ts = now_time - prev_time  #获取时间差
        if Ts==0:                  #防止程序卡死
            Ts = 0.001
        vel_targe = (X_targe - prev_X_targe)/Ts*0.01   #根据目标值变化速度得到电机大致的移动速度
        if abs(X_targe) > abs(prev_X_targe): #当前目标值比上一个目标值大，色块往图像外移动，加速追踪阶段不需要减速
            outout = X_targe
        else:
            if X_targe < 0.4:                #相机快要追到色块（色块往图像里移动），减速阶段，给非常大的减速环
                outout = X_targe + inertia_loop.operator(error=vel_targe)#减速环，速度越大，反向力矩越大
            else:                            #相机还有段距离才能追到色块，小减速阶段，给一个小的减速环
                outout = X_targe + Minertia_loop.operator(error=vel_targe)

        output_angle = angle_loop.operator(error=outout)#位置环，将减速环作为内环串联到位置环里
        output_str="%f"%output_angle                    #转为字符串
        motor_controller.send_target(0, output_str)     #串口发送目标位置给M1电机
        prev_time = now_time
        prev_X_targe = X_targe



